#include "lamatrix.h"

#include <windows.h>
#include <sstream>

LAMatrix::LAMatrix( int m, int n ) :
	m( m ),
	n( n )
{
	for( int i = 0, size = n*m; i < size; i++ )
		matrixData.push_back( 0.0f );
}

LAMatrix::~LAMatrix(void)
{
}

float& LAMatrix::operator()( int l, int c )
{
	if( l < 0 || l > m || c < 0 || c > n )
		throw std::exception();

	return matrixData[ l + c * m ];
}

void LAMatrix::mult( float* vector, float* out )
{
	for( int i = 0; i < m; i++ ) 
	{
		out[ i ] = 0.0f;
		for( int j = 0; j < n; j++ )
		{
			out[ i ] += (*this)( i, j ) * vector[ j ];
		}
	}
}

void LAMatrix::operator*=( LAMatrix& rhs )
{
	int resultM = m;
	int resultN = rhs.getN();

	std::vector< float > scrapMatrix;
	for( int i = 0; i < resultM * resultN; i++ )
		scrapMatrix.push_back( 0.0f );

	for( int i = 0; i < resultM; i++ ) 
	{
		for( int j = 0; j < resultN; j++ )
		{
			float v = 0.0f;

			for( int k = 0; k < n; k++ )
			{
				v += (*this)( i, k ) * rhs( k, j );
			}

			scrapMatrix[ i * resultN + j ] = v;
		}
	}

	for( int i = 0; i < resultM; i++ ) 
		for( int j = 0; j < resultN; j++ )
			(*this)( i, j ) = scrapMatrix[ i * resultN + j ];
}

void LAMatrix::operator*=( float a )
{
	for( int i = 0; i < m; i++ )
	{
		for( int j = 0; j < n ; j++ )
		{
			(*this)( i, j ) *= a;
		}
	}
}

void LAMatrix::operator+=( LAMatrix& matrix )
{
	if( m != matrix.getM() || n != matrix.getN() )
		throw std::exception();

	for( int i = 0; i < m; i++ )
	{
		for( int j = 0; j < n ; j++ )
		{
			(*this)( i, j ) += matrix( i, j );
		}
	}
}

void LAMatrix::invert()
{
	if( n != m )
		throw std::exception();

	int actualsize = m;
	int maxsize = m;

	std::vector< float > data;

	for( int i = 0; i < m; i++ )
		for (int j = 0; j < m ; j++)
			data.push_back( (*this)( j, i ) );

	for (int i=1; i < actualsize; i++) data[i] /= data[0]; // normalize row 0
	for (int i=1; i < actualsize; i++)  { 
		for (int j=i; j < actualsize; j++)  { // do a column of L
			float sum = 0.0;
			for (int k = 0; k < i; k++)  
				sum += data[j*maxsize+k] * data[k*maxsize+i];
			data[j*maxsize+i] -= sum;
		}
		if (i == actualsize-1) continue;
		for (int j=i+1; j < actualsize; j++)  {  // do a row of U
			float sum = 0.0;
			for (int k = 0; k < i; k++)
				sum += data[i*maxsize+k]*data[k*maxsize+j];
			data[i*maxsize+j] = 
				(data[i*maxsize+j]-sum) / data[i*maxsize+i];
		}
	}
	for ( int i = 0; i < actualsize; i++ )  // invert L
		for ( int j = i; j < actualsize; j++ )  {
			float x = 1.0;
			if ( i != j ) {
				x = 0.0;
				for ( int k = i; k < j; k++ ) 
					x -= data[j*maxsize+k]*data[k*maxsize+i];
			}
			data[j*maxsize+i] = x / data[j*maxsize+j];
		}
		for ( int i = 0; i < actualsize; i++ )   // invert U
			for ( int j = i; j < actualsize; j++ )  {
				if ( i == j ) continue;
				float sum = 0.0f;
				for ( int k = i; k < j; k++ )
					sum += data[k*maxsize+j]*( (i==k) ? 1.0f : data[i*maxsize+k] );
				data[i*maxsize+j] = -sum;
			}
			for ( int i = 0; i < actualsize; i++ )   // final inversion
				for ( int j = 0; j < actualsize; j++ )  {
					float sum = 0.0f;
					for ( int k = ((i>j)?i:j); k < actualsize; k++ )  
						sum += ((j==k)?1.0:data[j*maxsize+k])*data[k*maxsize+i];
					data[j*maxsize+i] = sum;
				}

	for( int i = 0; i < m; i++ )
		for (int j = 0; j < m ; j++)
			(*this)( j, i ) = data[ i + j * m ];
}

float LAMatrix::norm()
{
	float v = 0.0f;

	for (int i = 0; i < m ; i++)
		for (int j = 0; j < n ; j++)
			v += (*this)( i, j ) * (*this)( i, j );

	v = sqrt( v );
	return v;
}

float LAMatrix::det()
{
	if( n != m )
		throw std::exception();

	float** a = new float*[m];//(float**) malloc( m * sizeof( float* ) );
	for( int i = 0; i < m; i++ )
	{ 
		a[i] = new float[m];//(float*) malloc( m * sizeof( float* ) );
	}

	for (int i = 0; i < m*m; i++)
	{
		int l = i % m;
		int c = (i / m);
		a[l][c] = (*this)( l, c );
	}

	float det = internalDet( a, m );

	for( int i = 0; i < m; i++ )
		delete [] a[i];
	delete [] a;
		//free( a[i] );
	//free(a);

	return det;
}

float LAMatrix::internalDet( float**a, int n )
{
	int i,j,j1,j2;
	float det = 0;
	float **m = NULL;

	if (n < 1) { /* Error */

	} else if (n == 1) { /* Shouldn't get used */
		det = a[0][0];
	} else if (n == 2) {
		det = a[0][0] * a[1][1] - a[1][0] * a[0][1];
	} else {
		det = 0;
		for (j1=0;j1<n;j1++) {
			m = (float**) malloc((n-1)*sizeof(float *));
			for (i=0;i<n-1;i++)
				m[i] = (float*) malloc((n-1)*sizeof(float));
			for (i=1;i<n;i++) {
				j2 = 0;
				for (j=0;j<n;j++) {
					if (j == j1)
						continue;
					m[i-1][j2] = a[i][j];
					j2++;
				}
			}
			det += pow(-1.0f,j1+2.0f) * a[0][j1] * internalDet(m,n-1);
			for (i=0;i<n-1;i++)
				free(m[i]);
			free(m);
		}
	}
	return(det);
}

void LAMatrix::print()
{
	std::stringstream sstr;

	for (int i = 0; i < m ; i++)
	{
		sstr << "| ";
		for (int j = 0; j < n - 1 ; j++)
		{
			sstr << (*this)( i, j ) << "\t";
		}
		sstr << (*this)( i, n - 1 ) << "|\n";
	}
	sstr << "\n";
	OutputDebugString( (LPCTSTR) sstr.str().data() );
}

void LAMatrix::clear()
{
	for (int i = 0; i < m ; i++)
		for (int j = 0; j < n - 1 ; j++)
			(*this)( i, j ) = 0.0f;
}
